dgit.raspbian.org Git - pcl.git/blob

   1 /*
   2  * Software License Agreement (BSD License)
   3  *
   4  *  Point Cloud Library (PCL) - www.pointclouds.org
   5  *  Copyright (c) 2019-, Open Perception, Inc.
   6  *
   7  *  All rights reserved.
   8  *
   9  *  Redistribution and use in source and binary forms, with or without
  10  *  modification, are permitted provided that the following conditions
  11  *  are met:
  12  *
  13  *   * Redistributions of source code must retain the above copyright
  14  *     notice, this list of conditions and the following disclaimer.
  15  *   * Redistributions in binary form must reproduce the above
  16  *     copyright notice, this list of conditions and the following
  17  *     disclaimer in the documentation and/or other materials provided
  18  *     with the distribution.
  19  *   * Neither the name of the copyright holder(s) nor the names of its
  20  *     contributors may be used to endorse or promote products derived
  21  *     from this software without specific prior written permission.
  22  *
  23  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  24  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  25  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  26  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  27  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  28  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  29  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  30  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  31  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  32  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  33  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  34  *  POSSIBILITY OF SUCH DAMAGE.
  35  *
  36  */
  37
  38 #pragma once
  39
  40 #include <pcl/cloud_iterator.h>
  41
  42
  43 namespace pcl
  44 {
  45
  46 namespace registration
  47 {
  48
  49 template <typename PointSource, typename PointTarget, typename Scalar> inline void
  50 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
  51 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
  52                              const pcl::PointCloud<PointTarget> &cloud_tgt,
  53                              Matrix4 &transformation_matrix) const
  54 {
  55   const auto nr_points = cloud_src.points.size ();
  56   if (cloud_tgt.points.size () != nr_points)
  57   {
  58     PCL_ERROR ("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs from target (%lu)!\n", nr_points, cloud_tgt.points.size ());
  59     return;
  60   }
  61
  62   ConstCloudIterator<PointSource> source_it (cloud_src);
  63   ConstCloudIterator<PointTarget> target_it (cloud_tgt);
  64   estimateRigidTransformation (source_it, target_it, transformation_matrix);
  65 }
  66
  67
  68 template <typename PointSource, typename PointTarget, typename Scalar> void
  69 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
  70 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
  71                              const std::vector<int> &indices_src,
  72                              const pcl::PointCloud<PointTarget> &cloud_tgt,
  73                              Matrix4 &transformation_matrix) const
  74 {
  75   const auto nr_points = indices_src.size ();
  76   if (cloud_tgt.points.size () != nr_points)
  77   {
  78     PCL_ERROR ("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), cloud_tgt.points.size ());
  79     return;
  80   }
  81
  82   ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
  83   ConstCloudIterator<PointTarget> target_it (cloud_tgt);
  84   estimateRigidTransformation (source_it, target_it, transformation_matrix);
  85 }
  86
  87
  88 template <typename PointSource, typename PointTarget, typename Scalar> inline void
  89 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
  90 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
  91                              const std::vector<int> &indices_src,
  92                              const pcl::PointCloud<PointTarget> &cloud_tgt,
  93                              const std::vector<int> &indices_tgt,
  94                              Matrix4 &transformation_matrix) const
  95 {
  96   const auto nr_points = indices_src.size ();
  97   if (indices_tgt.size () != nr_points)
  98   {
  99     PCL_ERROR ("[pcl::TransformationEstimationSymmetricPointToPlaneLLS::estimateRigidTransformation] Number or points in source (%lu) differs than target (%lu)!\n", indices_src.size (), indices_tgt.size ());
 100     return;
 101   }
 102
 103   ConstCloudIterator<PointSource> source_it (cloud_src, indices_src);
 104   ConstCloudIterator<PointTarget> target_it (cloud_tgt, indices_tgt);
 105   estimateRigidTransformation (source_it, target_it, transformation_matrix);
 106 }
 107
 108
 109 template <typename PointSource, typename PointTarget, typename Scalar> inline void
 110 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
 111 estimateRigidTransformation (const pcl::PointCloud<PointSource> &cloud_src,
 112                              const pcl::PointCloud<PointTarget> &cloud_tgt,
 113                              const pcl::Correspondences &correspondences,
 114                              Matrix4 &transformation_matrix) const
 115 {
 116   ConstCloudIterator<PointSource> source_it (cloud_src, correspondences, true);
 117   ConstCloudIterator<PointTarget> target_it (cloud_tgt, correspondences, false);
 118   estimateRigidTransformation (source_it, target_it, transformation_matrix);
 119 }
 120
 121
 122 template <typename PointSource, typename PointTarget, typename Scalar> inline void
 123 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
 124 constructTransformationMatrix (const Vector6 &parameters,
 125                                Matrix4 &transformation_matrix) const
 126 {
 127   // Construct the transformation matrix from rotation and translation
 128   const Eigen::AngleAxis<Scalar> rotation_z (parameters (2), Eigen::Matrix<Scalar, 3, 1>::UnitZ ());
 129   const Eigen::AngleAxis<Scalar> rotation_y (parameters (1), Eigen::Matrix<Scalar, 3, 1>::UnitY ());
 130   const Eigen::AngleAxis<Scalar> rotation_x (parameters (0), Eigen::Matrix<Scalar, 3, 1>::UnitX ());
 131   const Eigen::Translation<Scalar, 3> translation (parameters (3), parameters (4), parameters (5));
 132   const Eigen::Transform<Scalar, 3, Eigen::Affine> transform = rotation_z * rotation_y * rotation_x *
 133                                                                translation *
 134                                                                rotation_z * rotation_y * rotation_x;
 135   transformation_matrix = transform.matrix ();
 136 }
 137
 138
 139 template <typename PointSource, typename PointTarget, typename Scalar> inline void
 140 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
 141 estimateRigidTransformation (ConstCloudIterator<PointSource>& source_it, ConstCloudIterator<PointTarget>& target_it, Matrix4 &transformation_matrix) const
 142 {
 143   using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
 144   using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
 145
 146   Matrix6 ATA;
 147   Vector6 ATb;
 148   ATA.setZero ();
 149   ATb.setZero ();
 150   auto M = ATA.template selfadjointView<Eigen::Upper> ();
 151
 152   // Approximate as a linear least squares problem
 153   source_it.reset ();
 154   target_it.reset ();
 155   for (; source_it.isValid () && target_it.isValid (); ++source_it, ++target_it)
 156   {
 157     const Vector3 p (source_it->x, source_it->y, source_it->z);
 158     const Vector3 q (target_it->x, target_it->y, target_it->z);
 159     const Vector3 n1 (source_it->getNormalVector3fMap().template cast<Scalar>());
 160     const Vector3 n2 (target_it->getNormalVector3fMap().template cast<Scalar>());
 161     Vector3 n;
 162     if (enforce_same_direction_normals_)
 163     {
 164         if (n1.dot (n2) >= 0.)
 165             n = n1 + n2;
 166         else
 167             n = n1 - n2;
 168     }
 169     else
 170     {
 171         n = n1 + n2;
 172     }
 173
 174     if (!p.array().isFinite().all() ||
 175         !q.array().isFinite().all() ||
 176         !n.array().isFinite().all())
 177     {
 178       continue;
 179     }
 180
 181     Vector6 v;
 182     v << (p + q).cross (n), n;
 183     M.rankUpdate (v);
 184
 185     ATb += v * (q - p).dot (n);
 186   }
 187
 188   // Solve A*x = b
 189   const Vector6 x = M.ldlt ().solve (ATb);
 190
 191   // Construct the transformation matrix from x
 192   constructTransformationMatrix (x, transformation_matrix);
 193 }
 194
 195
 196 template <typename PointSource, typename PointTarget, typename Scalar> inline void
 197 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
 198 setEnforceSameDirectionNormals (bool enforce_same_direction_normals)
 199 {
 200     enforce_same_direction_normals_ = enforce_same_direction_normals;
 201 }
 202
 203
 204 template <typename PointSource, typename PointTarget, typename Scalar> inline bool
 205 TransformationEstimationSymmetricPointToPlaneLLS<PointSource, PointTarget, Scalar>::
 206 getEnforceSameDirectionNormals ()
 207 {
 208     return enforce_same_direction_normals_;
 209 }
 210
 211 } // namespace registration
 212 } // namespace pcl
 213